#include "ros/ros.h"
#include "schunk_msgs/Positions.h"
#include "schunk_msgs/PositionsVector.h"

schunk_msgs::Positions position_end;
schunk_msgs::Positions position_ref;
schunk_msgs::Positions pose_end;
schunk_msgs::Positions pose_ref;

schunk_msgs::PositionsVector position_msg;
schunk_msgs::PositionsVector pose_msg;

ros::Publisher pub_position;
ros::Publisher pub_pose;

void poseEndCallback(const schunk_msgs::Positions::ConstPtr& msg)
{
    pose_end = *msg;
}

void poseRefCallback(const schunk_msgs::Positions::ConstPtr& msg)
{
    pose_msg.reference = *msg;
    pose_msg.real = pose_end;

    pub_pose.publish(pose_msg);
}

void positionEndCallback(const schunk_msgs::Positions::ConstPtr& msg)
{
    position_end = *msg;
}

void positionRefCallback(const schunk_msgs::Positions::ConstPtr& msg)
{
    position_msg.reference = *msg;
    position_msg.real = position_end;

    pub_position.publish(position_msg);
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "schunk_message_merger");
    ros::NodeHandle n;

    ros::Subscriber sub_position_ref = n.subscribe("schunk_high/position_controller/references", 1000, positionRefCallback);
    ros::Subscriber sub_position_end = n.subscribe("schunk_high/position_controller/end_effector_position", 1000, positionEndCallback);

    ros::Subscriber sub_pose_ref = n.subscribe("schunk_high/pose_controller/references", 1000, poseRefCallback);
    ros::Subscriber sub_pose_end = n.subscribe("schunk_high/pose_controller/endeffector_pose", 1000, poseEndCallback);

    pub_position = n.advertise<schunk_msgs::PositionsVector>("/schunk_high/merged_messages/position", 1000);
    pub_pose = n.advertise<schunk_msgs::PositionsVector>("/schunk_high/merged_messages/pose", 1000);

    ros::spin();

  return 0;
}
